% showPointCloud
%
% Function to diplay color point cloud from rgb and depth
%
% Usage: myShowPointcloud_adv(rgb_file, depth_file)
%
%        rgb   - rgb image file
%        depth - depth image file

function myShowPointcloud_adv(rgb_file, depth_file)

rgb = imread(rgb_file);
depth = imread(depth_file);

% Color image interior calibration parameter
rgb_cal = struct('fx', 5.2921508098293293e+02,'fy',5.2556393630057437e+02, 'cx', 3.2894272028759258e+02, 'cy', 2.6748068171871557e+02, 'k1', 2.6451622333009589e-01, 'k2', -8.3990749424620825e-01,'p1',-1.9922302173693159e-03, 'p2', 1.4371995932897616e-03, 'k3', 9.1192465078713847e-01);

% Depth image interior calibration parameter
depth_cal = struct('fx', 5.9421434211923247e+02,'fy',5.9104053696870778e+02, 'cx', 3.3930780975300314e+02, 'cy', 2.4273913761751615e+02, 'k1', -2.6386489753128833e-01, 'k2', 9.9966832163729757e-01,'p1', -7.6275862143610667e-04, 'p2', 5.0350940090814270e-03, 'k3', -1.3053628089976321e+00);

% Relative transform between the sensors (in meters) 
R = [ 9.9984628826577793e-01, 1.2635359098409581e-03, -1.7487233004436643e-02; -1.4779096108364480e-03, 9.9992385683542895e-01, -1.2251380107679535e-02; 1.7470421412464927e-02, 1.2275341476520762e-02, 9.9977202419716948e-01 ];
T = [ 1.9985242312092553e-02 ; -7.4423738761617583e-04 ; -1.0916736334336222e-02 ];

Dim = [480 640];
[rows cols] = ind2sub(Dim, 1:307200);

mat_rows = reshape(rows,480, 640);
mat_cols = reshape(cols,480, 640);

P_C = double(rgb);

P_C(:,:,3) = 1.0 ./ ( -0.0030711016 * double(depth) + 3.3309495161 );                % z
P_C(:,:,1) = (mat_cols - depth_cal.cx) .* P_C(:,:,3) / depth_cal.fx;                % x
P_C(:,:,2) = (mat_rows - depth_cal.cy) .* P_C(:,:,3) / depth_cal.fy ;               % y




P3D(:,1) = reshape(P_C(:,:,1), 307200, 1);
P3D(:,2) = reshape(P_C(:,:,2), 307200, 1);
P3D(:,3) = reshape(P_C(:,:,3), 307200, 1);

%P3D_(:,1) = (R(1,1)+R(2,1)+R(3,1)) * P3D(:,1) + T(1);           
%P3D_(:,2) = (R(1,2)+R(2,2)+R(3,2)) * P3D(:,2) + T(2);           
%P3D_(:,3) = (R(1,3)+R(2,3)+R(3,3)) * P3D(:,3) + T(3);    

%P2D_rgb_x = floor((P3D_(:,1) * rgb_cal.fx ./ P3D_(:,3)) + rgb_cal.cx);
%P2D_rgb_y = floor((P3D_(:,2) * rgb_cal.fy ./ P3D_(:,3)) + rgb_cal.cy);
            
%r_channel = double(rgb(P2D_rgb_x, P2D_rgb_y,1));
%g_channel = double(rgb(P2D_rgb_x, P2D_rgb_y,2));
%b_channel = double(rgb(P2D_rgb_x, P2D_rgb_y,3));
           
%Color_P3D(k,:) = [r_channel/255, g_channel/255, b_channel/255];         
%save 'CPC.mat' P3D Color_P3D;

plot3(P3D(:,1), P3D(:,2), P3D(:,3), 'b.');
xlabel('x');
ylabel('y');
zlabel('z');
grid on;

for i=1:180
	camroll(5)
	drawnow
end

end

